//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real>::Matrix3(bool bZero)
{
	if (bZero)
	{
		MakeZero();
	} 
	else
	{
		MakeIdentity();
	}
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real>::Matrix3(Real f00, Real f01, Real f02,
				 Real f10, Real f11, Real f12,
				 Real f20, Real f21, Real f22)
{
	m_afTuple[0] = f00;
	m_afTuple[1] = f01;
	m_afTuple[2] = f02;
	m_afTuple[3] = f10;
	m_afTuple[4] = f11;
	m_afTuple[5] = f12;
	m_afTuple[6] = f20;
	m_afTuple[7] = f21;
	m_afTuple[8] = f22;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real>::Matrix3(const Matrix3& rkM)
{
	m_afTuple[0] = rkM.m_afTuple[0];
	m_afTuple[1] = rkM.m_afTuple[1];
	m_afTuple[2] = rkM.m_afTuple[2];
	m_afTuple[3] = rkM.m_afTuple[3];
	m_afTuple[4] = rkM.m_afTuple[4];
	m_afTuple[5] = rkM.m_afTuple[5];
	m_afTuple[6] = rkM.m_afTuple[6];
	m_afTuple[7] = rkM.m_afTuple[7];
	m_afTuple[8] = rkM.m_afTuple[8];	
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real>::Matrix3(const Vector3<Real>& rkAxis, Real fAngleRad)
{
	FromAxisAngle(rkAxis, fAngleRad);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real>& Matrix3<Real>::MakeZero()
{
	m_afTuple[0] = (Real)0.0;
	m_afTuple[1] = (Real)0.0;
	m_afTuple[2] = (Real)0.0;
	m_afTuple[3] = (Real)0.0;
	m_afTuple[4] = (Real)0.0;
	m_afTuple[5] = (Real)0.0;
	m_afTuple[6] = (Real)0.0;
	m_afTuple[7] = (Real)0.0;
	m_afTuple[8] = (Real)0.0;
	return *this;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real>& Matrix3<Real>::MakeIdentity()
{
	m_afTuple[0] = (Real)1.0;
	m_afTuple[1] = (Real)0.0;
	m_afTuple[2] = (Real)0.0;
	m_afTuple[3] = (Real)0.0;
	m_afTuple[4] = (Real)1.0;
	m_afTuple[5] = (Real)0.0;
	m_afTuple[6] = (Real)0.0;
	m_afTuple[7] = (Real)0.0;
	m_afTuple[8] = (Real)1.0;
	return *this;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
int Matrix3<Real>::CompareArrays(const Matrix3& rkM) const
{
	return memcmp(m_afTuple, rkM.m_afTuple, 9*sizeof(Real));
}
//---------------------------------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::operator== (const Matrix3& rkM) const
{
	return CompareArrays(rkM) == 0;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::operator!= (const Matrix3& rkM) const
{
	return CompareArrays(rkM) != 0;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::operator< (const Matrix3& rkM) const
{
	return CompareArrays(rkM) < 0;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::operator<= (const Matrix3& rkM) const
{
	return CompareArrays(rkM) <= 0;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::operator> (const Matrix3& rkM) const
{
	return CompareArrays(rkM) > 0;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
bool Matrix3<Real>::operator>= (const Matrix3& rkM) const
{
	return CompareArrays(rkM) >= 0;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real>& Matrix3<Real>::FromAxisAngle(const Vector3<Real>& rkAxis, Real fAngleRad)
{
	Real fCos = (Real)cos((double)fAngleRad);
	Real fSin = (Real)sin((double)fAngleRad);
	Real fOneMinusCos = 1.0f - fCos;
	Real fX2 = rkAxis[0]*rkAxis[0];
	Real fY2 = rkAxis[1]*rkAxis[1];
	Real fZ2 = rkAxis[2]*rkAxis[2];
	Real fXYM = rkAxis[0]*rkAxis[1]*fOneMinusCos;
	Real fXZM = rkAxis[0]*rkAxis[2]*fOneMinusCos;
	Real fYZM = rkAxis[1]*rkAxis[2]*fOneMinusCos;
	Real fXSin = rkAxis[0]*fSin;
	Real fYSin = rkAxis[1]*fSin;
	Real fZSin = rkAxis[2]*fSin;	

	m_afTuple[0] = fX2*fOneMinusCos + fCos;
	m_afTuple[1] = fXYM + fZSin;
	m_afTuple[2] = fXZM - fYSin;

	m_afTuple[3] = fXYM - fZSin;
	m_afTuple[4] = fY2*fOneMinusCos + fCos;
	m_afTuple[5] = fYZM + fXSin;

	m_afTuple[6] = fXZM + fYSin;
	m_afTuple[7] = fYZM - fXSin;
	m_afTuple[8] = fZ2*fOneMinusCos + fCos;

	return *this;
}
//---------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Matrix3<Real>::operator Real* ()
{
	return m_afTuple;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Matrix3<Real>::operator const Real* () const
{
	return m_afTuple;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Real& Matrix3<Real>::operator[] (int iEntry)
{
	if (iEntry < 0)
	{
		iEntry = 0;
	}
	else if (iEntry > 8)
	{
		iEntry = 8;
	}
	return m_afTuple[iEntry];
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Vector3<Real> Matrix3<Real>::GetRow (int iRow) const
{
    int i0 = 3*iRow, i1 = i0+1, i2 = i1+1;
    return Vector3<Real>(m_afTuple[i0],m_afTuple[i1],m_afTuple[i2]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Vector3<Real> Matrix3<Real>::GetColumn (int iCol) const
{
    return Vector3<Real>(m_afTuple[iCol],m_afTuple[iCol+3],m_afTuple[iCol+6]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Matrix3<Real> Matrix3<Real>::operator* (const Matrix3& rkM) const
{
	return Matrix3(
		m_afTuple[0]*rkM.m_afTuple[0] +
		m_afTuple[1]*rkM.m_afTuple[3] +
		m_afTuple[2]*rkM.m_afTuple[6],

		m_afTuple[0]*rkM.m_afTuple[1] +
		m_afTuple[1]*rkM.m_afTuple[4] +
		m_afTuple[2]*rkM.m_afTuple[7],

		m_afTuple[0]*rkM.m_afTuple[2] +
		m_afTuple[1]*rkM.m_afTuple[5] +
		m_afTuple[2]*rkM.m_afTuple[8],

		m_afTuple[3]*rkM.m_afTuple[0] +
		m_afTuple[4]*rkM.m_afTuple[3] +
		m_afTuple[5]*rkM.m_afTuple[6],

		m_afTuple[3]*rkM.m_afTuple[1] +
		m_afTuple[4]*rkM.m_afTuple[4] +
		m_afTuple[5]*rkM.m_afTuple[7],

		m_afTuple[3]*rkM.m_afTuple[2] +
		m_afTuple[4]*rkM.m_afTuple[5] +
		m_afTuple[5]*rkM.m_afTuple[8],

		m_afTuple[6]*rkM.m_afTuple[0] +
		m_afTuple[7]*rkM.m_afTuple[3] +
		m_afTuple[8]*rkM.m_afTuple[6],

		m_afTuple[6]*rkM.m_afTuple[1] +
		m_afTuple[7]*rkM.m_afTuple[4] +
		m_afTuple[8]*rkM.m_afTuple[7],

		m_afTuple[6]*rkM.m_afTuple[2] +
		m_afTuple[7]*rkM.m_afTuple[5] +
		m_afTuple[8]*rkM.m_afTuple[8]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Matrix3<Real> Matrix3<Real>::operator* (Real fScalar) const
{
	return Matrix3(
		fScalar*m_afTuple[0],
		fScalar*m_afTuple[1],
		fScalar*m_afTuple[2],
		fScalar*m_afTuple[3],
		fScalar*m_afTuple[4],
		fScalar*m_afTuple[5],
		fScalar*m_afTuple[6],
		fScalar*m_afTuple[7],
		fScalar*m_afTuple[8]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Vector3<Real> Matrix3<Real>::operator* (const Vector3<Real>& rkV) const
{
	return Vector3<Real>(
		m_afTuple[0]*rkV[0] + m_afTuple[3]*rkV[1] + m_afTuple[6]*rkV[2],
		m_afTuple[1]*rkV[0] + m_afTuple[4]*rkV[1] + m_afTuple[7]*rkV[2],
		m_afTuple[2]*rkV[0] + m_afTuple[5]*rkV[1] + m_afTuple[8]*rkV[2]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Matrix3<Real> Matrix3<Real>::Transpose() const
{
	return Matrix3(
		m_afTuple[0],
		m_afTuple[3],
		m_afTuple[6],
		m_afTuple[1],
		m_afTuple[4],
		m_afTuple[7],
		m_afTuple[2],
		m_afTuple[5],
		m_afTuple[8]);	
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real> Matrix3<Real>::TransposeTimes (const Matrix3& rkM) const
{
    // P = A^T*B
    return Matrix3<Real>(
        m_afTuple[0]*rkM.m_afTuple[0] +
        m_afTuple[3]*rkM.m_afTuple[3] +
        m_afTuple[6]*rkM.m_afTuple[6],

        m_afTuple[0]*rkM.m_afTuple[1] +
        m_afTuple[3]*rkM.m_afTuple[4] +
        m_afTuple[6]*rkM.m_afTuple[7],

        m_afTuple[0]*rkM.m_afTuple[2] +
        m_afTuple[3]*rkM.m_afTuple[5] +
        m_afTuple[6]*rkM.m_afTuple[8],

        m_afTuple[1]*rkM.m_afTuple[0] +
        m_afTuple[4]*rkM.m_afTuple[3] +
        m_afTuple[7]*rkM.m_afTuple[6],

        m_afTuple[1]*rkM.m_afTuple[1] +
        m_afTuple[4]*rkM.m_afTuple[4] +
        m_afTuple[7]*rkM.m_afTuple[7],

        m_afTuple[1]*rkM.m_afTuple[2] +
        m_afTuple[4]*rkM.m_afTuple[5] +
        m_afTuple[7]*rkM.m_afTuple[8],

        m_afTuple[2]*rkM.m_afTuple[0] +
        m_afTuple[5]*rkM.m_afTuple[3] +
        m_afTuple[8]*rkM.m_afTuple[6],

        m_afTuple[2]*rkM.m_afTuple[1] +
        m_afTuple[5]*rkM.m_afTuple[4] +
        m_afTuple[8]*rkM.m_afTuple[7],

        m_afTuple[2]*rkM.m_afTuple[2] +
        m_afTuple[5]*rkM.m_afTuple[5] +
        m_afTuple[8]*rkM.m_afTuple[8]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real> Matrix3<Real>::TimesTranspose (const Matrix3& rkM) const
{
    // P = A*B^T
    return Matrix3<Real>(
        m_afTuple[0]*rkM.m_afTuple[0] +
        m_afTuple[1]*rkM.m_afTuple[1] +
        m_afTuple[2]*rkM.m_afTuple[2],

        m_afTuple[0]*rkM.m_afTuple[3] +
        m_afTuple[1]*rkM.m_afTuple[4] +
        m_afTuple[2]*rkM.m_afTuple[5],

        m_afTuple[0]*rkM.m_afTuple[6] +
        m_afTuple[1]*rkM.m_afTuple[7] +
        m_afTuple[2]*rkM.m_afTuple[8],

        m_afTuple[3]*rkM.m_afTuple[0] +
        m_afTuple[4]*rkM.m_afTuple[1] +
        m_afTuple[5]*rkM.m_afTuple[2],

        m_afTuple[3]*rkM.m_afTuple[3] +
        m_afTuple[4]*rkM.m_afTuple[4] +
        m_afTuple[5]*rkM.m_afTuple[5],

        m_afTuple[3]*rkM.m_afTuple[6] +
        m_afTuple[4]*rkM.m_afTuple[7] +
        m_afTuple[5]*rkM.m_afTuple[8],

        m_afTuple[6]*rkM.m_afTuple[0] +
        m_afTuple[7]*rkM.m_afTuple[1] +
        m_afTuple[8]*rkM.m_afTuple[2],

        m_afTuple[6]*rkM.m_afTuple[3] +
        m_afTuple[7]*rkM.m_afTuple[4] +
        m_afTuple[8]*rkM.m_afTuple[5],

        m_afTuple[6]*rkM.m_afTuple[6] +
        m_afTuple[7]*rkM.m_afTuple[7] +
        m_afTuple[8]*rkM.m_afTuple[8]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Matrix3<Real> Matrix3<Real>::DiagonalTimes(const Vector3<Real>& rkDiag) const
{
	return Matrix3(
		m_afTuple[0] * rkDiag[0], m_afTuple[1] * rkDiag[0], m_afTuple[2] * rkDiag[0],
		m_afTuple[3] * rkDiag[1], m_afTuple[4] * rkDiag[1], m_afTuple[5] * rkDiag[1],
		m_afTuple[6] * rkDiag[2], m_afTuple[7] * rkDiag[2], m_afTuple[8] * rkDiag[2]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Matrix3<Real> Matrix3<Real>::TimesDiagonal(const Vector3<Real>& rkDiag) const
{
	return Matrix3(
		m_afTuple[0] * rkDiag[0], m_afTuple[1] * rkDiag[1], m_afTuple[2] * rkDiag[2],
		m_afTuple[3] * rkDiag[0], m_afTuple[4] * rkDiag[1], m_afTuple[5] * rkDiag[2],
		m_afTuple[6] * rkDiag[0], m_afTuple[7] * rkDiag[1], m_afTuple[8] * rkDiag[2]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::Orthonormalize ()
{
    // Algorithm uses Gram-Schmidt orthogonalization.  If 'this' matrix is
    // M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
    //
    //   q0 = m0/|m0|
    //   q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
    //   q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
    //
    // where |V| indicates length of vector V and A*B indicates dot
    // product of vectors A and B.

    // compute q0
    Real fInvLength = Math<Real>::InvSqrt(m_afTuple[0]*m_afTuple[0] +
        m_afTuple[3]*m_afTuple[3] + m_afTuple[6]*m_afTuple[6]);

    m_afTuple[0] *= fInvLength;
    m_afTuple[3] *= fInvLength;
    m_afTuple[6] *= fInvLength;

    // compute q1
    Real fDot0 = m_afTuple[0]*m_afTuple[1] + m_afTuple[3]*m_afTuple[4] +
        m_afTuple[6]*m_afTuple[7];

    m_afTuple[1] -= fDot0*m_afTuple[0];
    m_afTuple[4] -= fDot0*m_afTuple[3];
    m_afTuple[7] -= fDot0*m_afTuple[6];

    fInvLength = Math<Real>::InvSqrt(m_afTuple[1]*m_afTuple[1] +
        m_afTuple[4]*m_afTuple[4] + m_afTuple[7]*m_afTuple[7]);

    m_afTuple[1] *= fInvLength;
    m_afTuple[4] *= fInvLength;
    m_afTuple[7] *= fInvLength;

    // compute q2
    Real fDot1 = m_afTuple[1]*m_afTuple[2] + m_afTuple[4]*m_afTuple[5] +
        m_afTuple[7]*m_afTuple[8];

    fDot0 = m_afTuple[0]*m_afTuple[2] + m_afTuple[3]*m_afTuple[5] +
        m_afTuple[6]*m_afTuple[8];

    m_afTuple[2] -= fDot0*m_afTuple[0] + fDot1*m_afTuple[1];
    m_afTuple[5] -= fDot0*m_afTuple[3] + fDot1*m_afTuple[4];
    m_afTuple[8] -= fDot0*m_afTuple[6] + fDot1*m_afTuple[7];

    fInvLength = Math<Real>::InvSqrt(m_afTuple[2]*m_afTuple[2] +
        m_afTuple[5]*m_afTuple[5] + m_afTuple[8]*m_afTuple[8]);

    m_afTuple[2] *= fInvLength;
    m_afTuple[5] *= fInvLength;
    m_afTuple[8] *= fInvLength;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
inline Vector3<Real> operator* (const Vector3<Real>& rkV, const Matrix3<Real>& rkM)
{
	return Vector3<Real>(
		rkV[0] * rkM[0] + rkV[1] * rkM[3] + rkV[2] * rkM[6],
		rkV[0] * rkM[1] + rkV[1] * rkM[4] + rkV[2] * rkM[7],
		rkV[0] * rkM[2] + rkV[1] * rkM[5] + rkV[2] * rkM[8]);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Matrix3<Real> Matrix3<Real>::Inverse () const
{
    // Invert a 3x3 using cofactors.  This is faster than using a generic
    // Gaussian elimination because of the loop overhead of such a method.

    Matrix3 kInverse;

    kInverse.m_afTuple[0] =
        m_afTuple[4]*m_afTuple[8] - m_afTuple[5]*m_afTuple[7];
    kInverse.m_afTuple[1] =
        m_afTuple[2]*m_afTuple[7] - m_afTuple[1]*m_afTuple[8];
    kInverse.m_afTuple[2] =
        m_afTuple[1]*m_afTuple[5] - m_afTuple[2]*m_afTuple[4];
    kInverse.m_afTuple[3] =
        m_afTuple[5]*m_afTuple[6] - m_afTuple[3]*m_afTuple[8];
    kInverse.m_afTuple[4] =
        m_afTuple[0]*m_afTuple[8] - m_afTuple[2]*m_afTuple[6];
    kInverse.m_afTuple[5] =
        m_afTuple[2]*m_afTuple[3] - m_afTuple[0]*m_afTuple[5];
    kInverse.m_afTuple[6] =
        m_afTuple[3]*m_afTuple[7] - m_afTuple[4]*m_afTuple[6];
    kInverse.m_afTuple[7] =
        m_afTuple[1]*m_afTuple[6] - m_afTuple[0]*m_afTuple[7];
    kInverse.m_afTuple[8] =
        m_afTuple[0]*m_afTuple[4] - m_afTuple[1]*m_afTuple[3];

    Real fDet =
        m_afTuple[0]*kInverse.m_afTuple[0] +
        m_afTuple[1]*kInverse.m_afTuple[3] +
        m_afTuple[2]*kInverse.m_afTuple[6];

    if (Math<Real>::FAbs(fDet) <= Math<Real>::ZERO_TOLERANCE)
    {
        return ZERO;
    }

    Real fInvDet = ((Real)1.0)/fDet;
    kInverse.m_afTuple[0] *= fInvDet;
    kInverse.m_afTuple[1] *= fInvDet;
    kInverse.m_afTuple[2] *= fInvDet;
    kInverse.m_afTuple[3] *= fInvDet;
    kInverse.m_afTuple[4] *= fInvDet;
    kInverse.m_afTuple[5] *= fInvDet;
    kInverse.m_afTuple[6] *= fInvDet;
    kInverse.m_afTuple[7] *= fInvDet;
    kInverse.m_afTuple[8] *= fInvDet;
    return kInverse;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Real Matrix3<Real>::Determinant () const
{
    Real fCo00 = m_afTuple[4]*m_afTuple[8] - m_afTuple[5]*m_afTuple[7];
    Real fCo10 = m_afTuple[5]*m_afTuple[6] - m_afTuple[3]*m_afTuple[8];
    Real fCo20 = m_afTuple[3]*m_afTuple[7] - m_afTuple[4]*m_afTuple[6];
    Real fDet = m_afTuple[0]*fCo00 + m_afTuple[1]*fCo10 + m_afTuple[2]*fCo20;
    return fDet;
}
//---------------------------------------------------------------------------------------------------
template <class Real>
Real Matrix3<Real>::QForm (const Vector3<Real>& rkU, const Vector3<Real>& rkV)
    const
{
    return rkU.Dot((*this)*rkV);
}
//---------------------------------------------------------------------------------------------------
template <class Real>
void Matrix3<Real>::Print()
{
	std::cout << std::endl;
	std::cout << m_afTuple[0] << "," << m_afTuple[1] << "," << m_afTuple[2] << std::endl
			  << m_afTuple[3] << "," << m_afTuple[4] << "," << m_afTuple[5] << std::endl
			  << m_afTuple[6] << "," << m_afTuple[7] << "," << m_afTuple[8] << std::endl;
}
//---------------------------------------------------------------------------------------------------